package sensors.virtualSensors;

import maze.Location;
import robots.OwnGhost;
import robots.Robot;
import robots.VirtualGhost;
import sensors.IRSensor;
import simulation.Raycaster;
import simulation.Simulator;

public class VirtualInfraredSensor extends VirtualSensor implements IRSensor{
	
	public VirtualInfraredSensor(VirtualGhost ghost, Simulator sim, boolean randomValues){
		super(ghost, sim,randomValues);
	}
	
	/**
	 * De direction wordt genummerd van links naar rechts.  Er zijn 9 mogelijke sectoren waarin pacman
	 * zich kan bevinden, gaande van 120° tot -120°
	 */
	@Override
	public int getDirection() {
		return simulateInfraredSensor();
	}
	/**
	 * De direction wordt genummerd van links naar rechts.  Er zijn 9 mogelijke sectoren waarin pacman
	 * zich kan bevinden, gaande van 120° tot -120°
	 */
	private int simulateInfraredSensor() {
		// beperking op de afstand die de sensor kan zien...
		int maxDistanceSensor = 50;//was 60

		/*
		 * rotatie 0 is naar rechts kijken 1 2 3 4 0 Jos 5 6 9 8 7
		 */
		Location pacmanLocation = getSimulator().getPacmanLocation();
		// verplaatsingsvector
		Location ghostLocation = getSimulator().getGhostLocations().get(getGhost());
		// double rico = (pacmanLocation.getY() -
		// ghostLocation.getY())/(pacmanLocation.getX() - ghostLocation.getX());
		// double angle = Math.atan(rico)*(180/Math.PI) +
		// getGhost().getRotation();
		// // bepalen we de sector waarin pacman zich bevind voor de
		// infraroodsensor
		// double sectorDouble = (-angle+120)/26.66667;
		// int sector = (int) (Math.ceil(sectorDouble));
		// //Vanaf hier proberen we de maxdistance uit te voeren en te
		// vergelijken met de afstand dat pacman echt is
		// // ten opzichte van de robot: dit op basis van afstandsberekening
		// tussen 2 punten
		// int IntAngle = (int) angle;
		// double SqrtXDist =
		// (pacmanLocation.getX()-ghostLocation.getX())*(pacmanLocation.getX()-ghostLocation.getX());
		// double SqrtYDist =
		// (pacmanLocation.getY()-ghostLocation.getY())*(pacmanLocation.getY()-ghostLocation.getY());
		// int distanceCalculated = (int) Math.sqrt(SqrtXDist + SqrtYDist);
		// int maxDistance = getGhost().getMaze().getMaxDistance(ghostLocation,
		// IntAngle);
		// if( maxDistance < distanceCalculated){
		// sector = 0;
		// }
		//
		// return sector;
		double diffx = pacmanLocation.x - ghostLocation.x;
		double diffy = pacmanLocation.y - ghostLocation.y;
		// verplaatsingsvector draaien (zoek op: "2d rotation matrix) om te
		// compenseren voor draaiing robot
		double counterAngle = -getSimulator().getGhostRotations().get(getGhost()) * Math.PI / 180.0; // in
																			// radialen
		double temp = Math.cos(counterAngle) * diffx - Math.sin(counterAngle) * diffy;
		double random = 0;
		if(randomizer != null)
			random = randomizer.infraredRandom() * Math.PI / 180.0;
		diffy = Math.sin(counterAngle) * diffx + Math.cos(counterAngle) * diffy + random;
		diffx = temp + random;
		// afstand uitrekenenen en bewaren, verplaatsingsvector normaliseren
		// (behoud de richting)
		double distance = Math.sqrt(diffx * diffx + diffy * diffy);
		diffx /= distance;
		diffy /= distance;
		int direction = 0;
		double dirWidth = 240.0 / 9.0;
		// bepalen van de richting
		if (diffx < Math.cos(120.0 * Math.PI / 180.0)) { // ~120
			direction = 0;
		} else if (diffx < Math.cos((120.0 - dirWidth) * Math.PI / 180.0)) { // ~84
			direction = (diffy > 0.0) ? 1 : 9;
		} else if (diffx < Math.cos((120.0 - dirWidth * 2) * Math.PI / 180.0)) { // ~58
			direction = (diffy > 0.0) ? 2 : 8;
		} else if (diffx < Math.cos((120.0 - dirWidth * 3) * Math.PI / 180.0)) { // ~31
			direction = (diffy > 0.0) ? 3 : 7;
		} else if (diffy > Math.sin((120.0 - dirWidth * 4) * Math.PI / 180.0)) { // overschakelen op sinus voor nauwkeurigheid
			direction = 4;
		} else if (diffy < Math.sin((120.0 - dirWidth * 5) * Math.PI / 180.0)) { // overschakelen op sinus voor nauwkeurigheid
			direction = 6;
		} else
			direction = 5;
		double angle = ((Math.abs(direction - 5) >= 3) ? Math.acos(diffx) : Math.asin(diffy)) * 180 / Math.PI;
		// calculate angle, kies cos of sin voor beste nauwkeurigheid
		if (Math.abs(direction - 5) >= 3) {
			if (Math.asin(diffy) < 0.0)
				angle = -angle;
		} else {
			if (Math.acos(diffx) < 0.0)
				angle = 180.0 - angle;
		}
		int maxDistance = Raycaster.cast(getSimulator().getMaze(),ghostLocation, (int) angle + getSimulator().getGhostRotations().get(getGhost()));
		if (maxDistance < distance || maxDistanceSensor < distance) {
			direction = 0;
		}
		return direction;
	}
}
